program Small_code_one_resonance
use my_own_little_code_utilities
use with_fpp
implicit none
type(my_taylor) Z(2),r2,x,p,PHASE_ADVANCE,INVARIANT
type(my_taylor)  x2_average,x_average,x_average_xp
type(normalform) NORMAL
type(normalform_fpp) NORMAL_fpp
type(inverse_dragt_finn) idf
type(my_map) M,id,a_cs,disp,A_l,A_nl
real(dp) L,k1,fix(3),ma(2,2),h,dx_average_dj
complex(dp) zz
integer i,n,mf,j,i1,i2,k,nst
integer beta(2),twoalpha(2),gamma(2)
type(magnet) :: lattice(6*10)
type(ray) r
type(damap) mz,mt
type(dragtfinn) df
!   mad-x lattice
!   L : drift, L= 1.0;
!   QF   : QUADRUPOLE, L=1.0, K1=1.0;  
!   QD   : QUADRUPOLE, L=1.0, K1=-1.0; 
!   sf  : sextupole,  K2= 2.0;
!s1 : LINE=  10*(QF,sf,L,QD,sf,L);

Write(6,*) " Creating the simple lattice: 10*(QF,sf,L,QD,sf,L) "
nst=100
L=1.0_dp
h=  twopi/20/L
do i=1,10
 k=(i-1)*6
LATTICE(1+k)%NAME="QF"
 LATTICE(1+k)%L=L
 LATTICE(1+k)%BN=0.0_DP
 LATTICE(1+k)%BN(0)=h
 LATTICE(1+k)%h=h
 LATTICE(1+k)%BN(1)=1.0_DP
 LATTICE(1+k)%N=nst
LATTICE(2+k)%NAME="SF"
 LATTICE(2+k)%L=0.0_DP
 LATTICE(2+k)%BN=0.0_DP
 LATTICE(2+k)%BN(2)=2.0_DP
 LATTICE(2+k)%h=0
 LATTICE(2+k)%N=0
LATTICE(3+k)%NAME="L"
 LATTICE(3+k)%L=1.0_DP
 LATTICE(3+k)%BN=0.0_DP
 LATTICE(3+k)%h=0
 LATTICE(3+k)%N=1
LATTICE(4+k)%NAME="QD"
 LATTICE(4+k)%L=L
 LATTICE(4+k)%BN=0.0_DP
 LATTICE(4+k)%BN(1)=-1.0_DP
 LATTICE(4+k)%BN(0)=h
 LATTICE(4+k)%h=h
 LATTICE(4+k)%N=nst
LATTICE(5+k)%NAME="SF"
 LATTICE(5+k)%L=0.0_DP
 LATTICE(5+k)%BN=0.0_DP
 LATTICE(5+k)%BN(2)=2.0_DP
 LATTICE(5+k)%h=0
 LATTICE(5+k)%N=0
LATTICE(6+k)%NAME="L"
 LATTICE(6+k)%L=1.0_DP
 LATTICE(6+k)%BN=0.0_DP
 LATTICE(6+k)%h=0
 LATTICE(6+k)%N=1
enddo
 
lattice(59)%bn(2)=20.0_dp 
 
my_order=5
call init(my_order,1,1,0)
delta_is_3rd_parameter=.true.

fix=0.0_dp  ! fixed point
call find_closed_orbit( fix,LATTICE,1)
write(6,*) " fix point = ",fix(1:2)
write(6,*) " delta = ",fix(3)

id=1
r%z=fix+id ! initialize the ray as --> ray = Fixed point + Identity (Taylor map)



!  Computing a one-turn map to order my_order

 CALL track_lattice( r,LATTICE,1,1)
 
m=r%z    

call alloc(mz,mt)
call alloc(NORMAL_fpp)
call alloc(df)

call convertmap_to_fpp(m,mz)

NORMAL_fpp=mz

NORMAL=M    ! normalizing the map


call kanalnummer(mf,"res.txt")
!call print(NORMAL%r,mf)
!NORMAL_fpp%a_t=NORMAL_fpp%normal
!call print(NORMAL_fpp%a_t,mf)

!close(mf)
!pause 8
!stop

call print(NORMAL%a_t,mf)
NORMAL_fpp%a_t=NORMAL_fpp%a_t.cut.5
call print(NORMAL_fpp%a_t,mf)
call convertmap_to_fpp(NORMAL%a_t,mz)
mz=mz**(-1)*NORMAL_fpp%a_t
write(mf,*) " rot? "
call print(mz,mf)
df=mz

mt=1
mt=exp(df%pb,mt)
write(mf,*) " pb : rot with dragt finn "
call print(df%pb,mf)
write(mf,*) " rot with dragt finn "
call print(mt,mf)


call convertmap_to_fpp(m,mz)



mz=NORMAL_fpp%a_t**(-1)*mz*NORMAL_fpp%a_t

write(mf,*) " rots "

call print(mz,mf)
NORMAL_fpp=mz
m=NORMAL%a_t**(-1)*m*NORMAL%a_t
call print(m,mf)
call convertmap_to_fpp(m,mt)
mt%v(1)=mt%v(1)-mz%v(1)
mt%v(2)=mt%v(2)-mz%v(2)
write(mf,*) " diff rots "

call print(mt,mf)

NORMAL=m


write(mf,*) " again "
call print(NORMAL_fpp%a_t,mf)
call print(NORMAL%a_t,mf)

close(mf)
pause 8
stop
!allocate(mres(1))
!mres(1)=3

NORMAL=M    ! normalizing the map

write(6,*) normal%tune



epsclean=1.e-4

call print(normal%a_t,6)
pause 1

m=normal%a_t**(-1)*m*normal%a_t

m=to_phasor*m*from_phasor 
call print(m,6)
pause 8
stop
!deallocate(mres)
 
NORMAL=M    ! normalizing the map


call print(normal%a_t,6)
pause 2

m=normal%a_t**(-1)*m*normal%a_t

NORMAL=M    ! normalizing the map
call print(normal%a_t,6)
pause 3

mf=10




end program Small_code_one_resonance
